import matplotlib.pyplot as plt
import numpy as np
import math

T = 10
delta = 0.1
t_0 = np.arange(0, T, 0.01)  #未采样前连续时间
XK = [0] * len(t_0)
#print(len(t_0))
for i in range(0, len(t_0)):
    XK[i] = 5 * (t_0[i]**2) - 2*t_0[i] + 2

K = list(range(1, int(T/ delta) + 1))
wk = [0] * len(K)
x_measure = [0] * len(K)
x_hat = [0] * len(K)
t = [0] * len(K)
e1 = [0] * len(K)
e2 = [0] * len(K)
a = [[0], [0]]
#计算测量值
for i in range(0, len(K)):
    t[i] = (K[i] - 1) * delta
    wk[i] = np.random.normal(loc=0.0, scale= 5.0, size=None)
    x_measure[i] = (5 * (t[i]**2) - 2*t[i] + 2) + wk[i]

#计算估计值
#一阶最小二乘滤波器,估计值的计算
m1 = np.array([[len(K),sum(t)],[sum(t),sum(list(map(lambda x:x**2,t)))]])
                                     #最后一个求和符号里面是求数列个元素平方
w = [t*x_measure for t,x_measure in zip(t,x_measure)]
m2 = np.array([[sum(x_measure)],[sum(w)]])

a = (np.linalg.inv(m1)).dot(m2)      #m1求逆再乘以m2
a0 = float(a[0])
a1 = float(a[1])

for i in range(0, len(K)):
    x_hat[i] = a0 + t[i] * a1
    e1[i] = x_hat[i] - x_measure[i]
    e2[i] = x_hat[i] - (5 * (t[i]**2) - 2*t[i] + 2)

plt.xlabel("Time(Sec)")
plt.ylabel("xhat")
plt.plot(t,x_measure,'-o',c = 'r',label = 'measures')
plt.plot(t,x_hat, c="k",label = 'estimates')
plt.legend()
plt.show()

plt.xlabel("Time(Sec)")
plt.plot(t,e1,'-o',label = 'error between estimates and measures')
plt.plot(t,e2,label = 'error between estimates and true')
plt.legend()
plt.show()

plt.xlabel("Time(Sec)")
plt.plot(t_0,XK, c="r",label = 'true',ls="--", lw=2)
plt.plot(t,x_hat,label = 'estimates')
plt.xlim(0,T)
plt.legend()
plt.show()